﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Numerics;
using System.Text;

namespace QuadRemote.Tabs
{
    public class BodeMeasurement
    {
        public float freq;

        public float Error_real;
        public float Error_im;
        public float Out_real;
        public float Out_im;
        public float In_real;
        public float In_im;
        public float Signal_real;
        public float Signal_im;

        private Boolean calculated;
        private float[] calcValues = new float[6];

        public BodeMeasurement()
        {
            for (int i = 0; i < 6; i++)
                calcValues[i] = 0;
        }

        public String getLine()
        {
            return freq + ";" +
                    Error_real + ";" + Error_im + ";" +
                    Out_real + ";" + Out_im + ";" +
                    In_real + ";" + In_im + ";" +
                    Signal_real + ";" + Signal_im + ";" +
                    getValue(0) + ";" + getValue(3) + ";" +
            getValue(1) + ";" + getValue(4) + ";" +
            getValue(2) + ";" + getValue(5);

        }
        public void hackSetGainPhase(int displayType, float g, float p)
        {
            calcValues[displayType] = g;
            calcValues[displayType + 3] = p;
            calculated = true;
        }

        public float getValue(int displayType)
        {
            if (!calculated) calculate();
            return calcValues[displayType];
        }

        private void calculate()
        {
            Complex cOut = new Complex(Out_real, -Out_im);
            Complex cIn = new Complex(-In_real, In_im);
            Complex cSig = new Complex(Signal_real * -1, Signal_im);
            Complex cErr = new Complex(-Error_real, Error_im);

            Complex c = cOut / cIn;

            calcValues[0] = 20 * (float)Math.Log10(c.Magnitude);
            calcValues[3] = (float)(c.Phase / Math.PI * 180);

            c = cOut / cSig;

            calcValues[1] = 20 * (float)Math.Log10(c.Magnitude);
            calcValues[4] = (float)(c.Phase / Math.PI * 180);

            Complex c2 = cErr / cSig;
            c = cIn / cSig;

            c = c2 / c;

            calcValues[2] = 20 * (float)Math.Log10(c.Magnitude);
            calcValues[5] = (float)(c.Phase / Math.PI * 180);

            calculated = true;
        }

        private float unwrapDiff(float own, float prev)
        {
            //return own;
            float orgDistance = Math.Abs(own - prev);

            while (Math.Abs(own - 360 - prev) < orgDistance)
            {
                own -= 360;
                orgDistance = Math.Abs(own - prev);
            }

            while (Math.Abs(own + 360 - prev) < orgDistance)
            {
                own += 360;
                orgDistance = Math.Abs(own - prev);
            }

            return own;
        }

        public void unwrap(BodeMeasurement prev)
        {
            for (int i = 0; i < 3;i++)
                calcValues[i+3] = unwrapDiff(getValue(i+3), prev.getValue(i+3));
        }

        public void addPhase(int displayType, int periods)
        {
            calcValues[displayType] += periods * 360;
        }
    }
}
